/**
 * @file:          MT6835_21bit.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.09
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.09,18:31:30
 */
/* Include Files **************************************************************/
#include "MT6835_21bit.h"
#include "stdio.h"
#include "common.h"
#include "ch32v30x_module.h"
#include <stdlib.h>
#include <string.h>

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
uint8 MT6835_21bit_Init(struct MT68xx *self)
{
    self->SingleToMultiTurns = MT6835_21bit_SingleToMultiTurns;
    self->GetEncoder = MT6835_21bit_GetEncoder;
    self->EncoderRawRead = MT6835_21bit_EncoderRawRead;
    self->GetEncoderCalibrated = MT6835_21bit_GetEncoderCalibrated;
    self->GetEncoderSingleturn = MT6835_21bit_GetEncoderSingleturn;
    self->GetEncoderCalibrationStatus = MT6835_21bit_GetEncoderCalibrationStatus;
    self->WriteReg = MT6835_21bit_WriteReg;
    self->ReadReg = MT6835_21bit_ReadReg;
    self->SaveIntoEEPROM = MT6835_21bit_SaveIntoEEPROM;

    if (self->raw_data != NULL)
    {
        free(self->raw_data);
    }
    self->raw_data = (void *)malloc(MT6835_PACKAGE_BYTES);

    // self->calibrator.Init(&self->calibrator);

    /* init all value related to encoder */
    self->GetEncoderCalibrated(self);
    self->GetEncoderSingleturn(self);
    self->GetEncoderSingleturn(self);

    // self->GetEncoder(self);
    // self->encoder_turns = 0;
    // self->GetEncoder(self);

    return TRUE;
}

int32 MT6835_21bit_SingleToMultiTurns(struct MT68xx *self)
{
    int32 err = 0;

    err = self->encoder_pulse_single_turn - self->encoder_pulse_single_turn_last;

    if (err < -(int32)(self->param_encoder_max_pulse >> 1))
    {
        self->encoder_turns++;
    }
    else if (err > (int32)(self->param_encoder_max_pulse >> 1))
    {
        self->encoder_turns--;
    }

    self->encoder_pulse_multi_turns = (!self->param_single_turn_mode) * self->encoder_turns * self->param_encoder_max_pulse + self->encoder_pulse_single_turn;

    return self->encoder_pulse_multi_turns;
}

int32 MT6835_21bit_EncoderRawRead(struct MT68xx *self)
{
    union MT6835_EncoderDataFrame *raw_data = (union MT6835_EncoderDataFrame *)self->raw_data;

    uint16 write_buff[4] = {
        ((MT6835_CMD_READ_REG | MT6835_REG_ANGLE_20to13) >> 8) & 0xff,
        (MT6835_CMD_READ_REG | MT6835_REG_ANGLE_20to13) & 0xff,
    };
    // DBprintf("-\r\n");
    // DBprintf("%#x\r\n", write_buff[0]);
    // DBprintf("%#x\r\n", write_buff[1]);
    // DBprintf("%#x\r\n", write_buff[2]);
    // DBprintf("%#x\r\n", write_buff[3]);
    // DBprintf("%#x\r\n", write_buff[4]);
    // uint16 read_buff[3] = {0};

    if (!self->param_fake_encoder_en)
    {
        // uint16 raw_data = 0;

        self->mcu->SPI_WriteReadData(write_buff, raw_data->u8data, 4, 100);
        self->varify_pass = 1;
        
        // raw_data = ((uint16)raw_data->u8data[1] << 8) | (uint16)raw_data->u8data[0];

        // DBprintf("%d\r\n",raw_data->bits.angle_pulse);

        /* varity 1.18us */
        // uint16 h_count = 0;
        // for (uint8_t j = 0; j < 16; j++)
        // {
        //     if (raw_data & (0x0001 << j))
        //         h_count++;
        // }
        // if (h_count & 0x01)
        // {
        //     self->varify_pass = 0;
        //     // DBprintf("[ERROR]:varity:%d\r\n", self->varify_pass);
        // }
        // else
        // {
        //     self->varify_pass = 1;
        // }

        // if (raw_data->bits.mag_warning != 0)
        // {
        //     // DBprintf("[WARNING]:mag:%d\r\n", raw_data->bits.mag_warning);
        // }
    }
    else
    {
        float32 encoder_last_f = 0;

        encoder_last_f = self->fake_encoder;
        self->fake_encoder = fmodf(self->fake_encoder + self->param_fake_encoder_step, self->param_encoder_max_pulse);

        /* prevent float point accumulate error */
        if (fabsf((int32)self->fake_encoder - (int32)encoder_last_f) != 0)
        {
            if (fabsf((int32)self->fake_encoder - encoder_last_f) < fabsf(self->param_fake_encoder_step * 0.1))
            {

                self->fake_encoder = (int32)self->fake_encoder + self->param_fake_encoder_step;
            }
            else
            {

                self->fake_encoder = (int32)self->fake_encoder;
            }
        }

        raw_data->bits.angle = (uint32)(self->fake_encoder >= 0 ? self->fake_encoder : self->param_encoder_max_pulse + self->fake_encoder);
        // raw_data->bits.mag_warning = 0;
        // DBprintf("%d %s\r\n", raw_data->bits.angle_pulse, float2str(self->fake_encoder, 2));
    }
    // DBprintf("%d\r\n", raw_data->bits.angle>>1);
    return raw_data->bits.angle;
}

int32 MT6835_21bit_GetEncoder(struct MT68xx *self)
{
    /*  */
    self->GetEncoderCalibrated(self);

    /* 0.2us */
    self->GetEncoderSingleturn(self);

    /* 0.1325us */
    self->SingleToMultiTurns(self);

    return self->encoder_pulse_multi_turns;
}

int32 MT6835_21bit_GetEncoderCalibrated(struct MT68xx *self)
{
    self->encoder_raw = self->EncoderRawRead(self);

    /* 0.2us */
    // self->encoder_used = (self->param_encoder_max_pulse + self->calibrator.Run(&self->calibrator, self->encoder_raw)) % self->param_encoder_max_pulse;
    self->encoder_used = self->encoder_raw % self->param_encoder_max_pulse;

    return self->encoder_used;
}

int32 MT6835_21bit_GetEncoderSingleturn(struct MT68xx *self)
{
    self->encoder_pulse_single_turn_last = self->encoder_pulse_single_turn;

    if (self->param_direction < 0)
    {
        self->encoder_pulse_single_turn = self->param_encoder_max_pulse - self->encoder_used;
    }
    else
    {
        self->encoder_pulse_single_turn = self->encoder_used;
    }

    // self->encoder_pulse_single_turn = (self->param_encoder_max_pulse + self->param_direction * self->encoder_used) % self->param_encoder_max_pulse;
    return self->encoder_pulse_single_turn;
}

uint8_t MT6835_21bit_GetEncoderCalibrationStatus(struct MT68xx *self)
{
    uint8 read_buff[3] = {0};

    uint16 write_buff[3] = {
        ((MT6835_CMD_READ_REG | MT6835_REG_CLIBRATION_STATUS) >> 8) & 0xff,
        (MT6835_CMD_READ_REG | MT6835_REG_CLIBRATION_STATUS) & 0xff,
    };

    self->mcu->SPI_WriteReadData(write_buff, read_buff, 3, 100);

    return read_buff[0];
}

uint16_t MT6835_21bit_WriteReg(struct MT68xx *self, uint16_t reg_addr, uint8_t *data_buff, uint8_t length)
{
    // uint8 read_buff[6] = {0};

    uint16 write_buff[6] = {0};
    uint8 read_buff[6] = {0};
    
    write_buff[0] = ((MT6835_CMD_WRITE_REG | reg_addr) >> 8) & 0xff;
    write_buff[1] = (MT6835_CMD_WRITE_REG | reg_addr) & 0xff;
    
    for (size_t i = 0; i < length; i++)
    {
        write_buff[2+i] = data_buff[i];
    }

    self->mcu->SPI_WriteReadData(write_buff, read_buff, 2+length, 100);

    return 1;
}

uint16_t MT6835_21bit_ReadReg(struct MT68xx *self, uint16_t reg_addr, uint8_t *data_buff, uint8_t length)
{
    // uint8 read_buff[6] = {0};

    uint16 write_buff[6] = {0};
    uint8 read_buff[6] = {0};
    
    write_buff[0] = ((MT6835_CMD_READ_REG | reg_addr) >> 8) & 0xff;
    write_buff[1] = (MT6835_CMD_READ_REG | reg_addr) & 0xff;
    
    // for (size_t i = 0; i < length; i++)
    // {
    //     write_buff[2+i] = data_buff[i];
    // }

    self->mcu->SPI_WriteReadData(write_buff, read_buff, 2+length, 100);

    memcpy(data_buff, read_buff, 2+length);

    return 1;
}

uint16_t MT6835_21bit_SaveIntoEEPROM(struct MT68xx *self)
{
    uint8 read_buff[3] = {0};

    uint16 write_buff[3] = {
        ((MT6835_CMD_WRITE_EEPROM) >> 8) & 0xff,
        (MT6835_CMD_WRITE_EEPROM) & 0xff,
    };

    self->mcu->SPI_WriteReadData(write_buff, read_buff, 3, 100);

    return read_buff[0];
}